TP2 - Filtros de Kalman

Elementos de Matemáticas Aplicadas para Aplicaciones Tecnológicas

Autor/a

Afiliación

Matías Roqueta

Instituto Balseiro

Descripción del problema

Se implementará un filtro de Kalman para fusionar los datos de sensores presentes en un vehículo para estimar la posición y orientación del mismo en dos dimensiones. Un diagrama del mismo se presenta en la Figura 1.

Figura 1: Esquema del sistema.

En la figura se representan los ejes \(x\) e \(y\), llamados el sistema de referencia inercial. Se repesenta además el eje \(x_b\), el cual junto con un eje \(y_b\) ortogonal al mismo forman el sistema de referencia del móvil, llamado body. El filtro de Kalman estimará la posición del móvil en el sistema de referencia intercial, \(x_{est},\, y_{est}\) así como la orientación del mismo, dada por el ángulo \(\theta\).

Los instrumentos disponibles para la estimación de la posición son los siguientes.

• Unidad de Medición Inercial (IMU): Consiste en un giróscopo y un acelerómetro los cuales miden a una tasa \(f_s\).
  • Acelerómetro: Mide la aceleración lineal del móvil en el sistema body, \(u^{x_b} = \ddot{x}_b\) y \(u^{y_b} = \ddot{y}_b\).
  • Giróscopo: Mide la velocidad angular del móvil, \(u^{\theta} = \dot{\theta}\).
• GPS: Mide la posición del móvil en el sistema de referencia inercial a una tasa \(f_s'\).
• Magnetómetro: Por medio de mediciones del campo magnético terrestre calcula la orientación del móvil, también a tasa \(f_s'\).

Error de los instrumentos

Para cada dimensión de medición se considera su correspondiente error

using LinearAlgebra
using Distributions
using Plots
using PlotThemes

theme(:wong2)

## Parametros de error instrumentos

Δt = 0.1

fs = 10
fs2 = 1
PSD_a = 80*10*1e-6
PSD_g = 0.03*π/180
PSD_m = 100e-6*1e-4
B_T = 25358e-9

err_IMU_x = PSD_a*sqrt(fs)
err_IMU_θ = PSD_g*sqrt(fs)

err_GPS = 2.5
err_MAG = PSD_m*sqrt(fs2)/B_T

Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]

R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

Filtro de Kalman

El filtro de Kalman se hará sobre el estimador del estado de posición, velocidad, y orientación del móvil en el sistema de referencia inercial

\[ \mathbf{\hat{x}}_k = \begin{bmatrix}\hat{x}_k \\ \hat{\dot{x}}_k \\ \hat{y}_k \\ \hat{\dot{y}}_k \\ \hat{\theta}_k \end{bmatrix} \]

Medición del IMU

A cada instante de muestreo del IMU se obtiene un vector de mediciones \(\mathbf{\overline{u}}_k\), el cual se define como su valor real \(\mathbf{u}_k\) más su error \(\mathbf{w'}_k\)

\[ \mathbf{\overline{u}}_k = \mathbf{u}_k + \mathbf{w}_k' = \begin{bmatrix}u_k^{x_b} \\ u_k^{y_b} \\ u_k^{\theta} \end{bmatrix} + \mathbf{w}_k' \]

Los errores de medición se consideran independientes, por lo que la matriz de covarianza de \(\mathbf{w}'\) es dada por

\[ Q = \begin{bmatrix} (\sigma^{x_b})^2 & 0 & 0\\ 0 & (\sigma^{x_b})^2 & 0\\ 0 & 0 & (\sigma^\theta)^2\\ \end{bmatrix} \]

Medición del GPS/Magnetómetro

Cuando \(k\) corresponde a un instante de muestreo del GPS, se obtiene un vector de mediciones \(\mathbf{\overline{z}}_k\), el cual se define como su valor real \(\mathbf{z}_k\) más su error \(\mathbf{v}_k\)

\[ \mathbf{\overline{z}}_k = \mathbf{z}_k + \mathbf{v}_k = \begin{bmatrix}z_k^{x} \\ z_k^{y} \\ z_k^{\theta} \end{bmatrix} + \mathbf{v}_k \]

En ausencia de aceleración, la evolución del estado en un tiempo \(\Delta t\) seguiría un movimiento rectilíneo uniforme, dado por

\[ \mathbf{x}_k = \begin{bmatrix}x_k \\ \dot{x}_k \\ y_k \\ \dot{y}_k \\ \theta_k \end{bmatrix} = \begin{bmatrix} 1 & \Delta t & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & \Delta t & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix}x_{k-1} \\ \dot{x}_{k-1} \\ y_{k-1} \\ \dot{y}_{k-1} \\ \theta_{k-1} \end{bmatrix} \]

Modelo de actualización de estado

La actualización del estado se hace en función de las mediciones de la IMU, que informan la aceleración del móvil en el sistema de referencia body, estas se modelan como el vector de medición \(\mathbf u\) más el vector de error \(\mathbf w'\)

Estas aceleraciones se transladan al sistema de referencia inercial y se utilizan para realizar la actualización por modelo del filtro de Kalman, dada por medio de la ecuación

\[ \mathbf{\hat{x}}_k = A\mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{u}_{k-1} + \mathbf{w}_{k-1} \]

En donde la matriz \(B_{k-1}\) que obtiene la actualización del estado en el sistema inercial \(\mathbf{x}\) en función de las mediciones de aceleración en el sistema body es dada por

\[ B_{k-1} = \begin{bmatrix} \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & - \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & 0 \\ \Delta t \cos(\theta_{k-1}) & - \Delta t \sin(\theta_{k-1}) & 0 \\ \frac 1 2 \Delta t^2 \sin(\theta_{k-1}) & \frac 1 2 \Delta t^2 \cos(\theta_{k-1}) & 0 \\ \Delta t \sin(\theta_{k-1}) & \Delta t \cos(\theta_{k-1}) & 0 \\ 0 & 0 & \Delta t \end{bmatrix} \]

Y el ruido es similarmente transformado, obteniendo \(\mathbf{w}_{k-1} = B \mathbf{w}_{k-1}'\). La matriz de covarianza que resulta de la transformación es dada por

\[ Q_k = B Q B^T \]

Modelo de actualización por medición

A = [1 Δt 0 0 0;
     0 1 0 0 0;
     0 0 1 Δt 0;
     0 0 0 1 0;
     0 0 0 0 1]

function B_matrix(θ)
    return [Δt^2*cos(θ)/2 -Δt^2*sin(θ)/2 0;
    Δt*cos(θ) -Δt*sin(θ) 0;
    Δt^2*sin(θ)/2 Δt^2*cos(θ)/2 0;
    Δt*sin(θ) Δt*cos(θ) 0;
    0 0 Δt]
end

H = [1 0 0 0 0;
     0 0 1 0 0;
     0 0 0 0 1]

Implementación del algoritmo de estimación

Inicialización

function kalman_initialize(t, z)
    N = length(t)
    x = zeros(5, N)
    x[1:2:5,1] = z[:,1]
    P = zeros(5, 5, N)
    P[:,:,1] = diagm([err_GPS^2,(err_GPS*fs2)^2,err_GPS^2,(err_GPS*fs2)^2,2pi])
    K = zeros(5, 3, N)
    return x, P, K
end

kalman_initialize (generic function with 1 method)

Predicción

En cada instante de muestreo del IMU se realiza la etapa de predicción

• Estimación a priori: \(\mathbf{\hat{x}}_k^{-} = \mathbf{\hat{x}}_{k-1} + B_{k-1} \mathbf{\overline{u}}_{k-1}\)
• Covarianza a priori: \(P_k^{-} = A P_{k-1}A^T + Q_k\)
• Ganancia de Kalman: \(K_k = P^{-}_k H^T (H P_k^{-}H^T + R_k)^{-1}\)

function kalman_predict(x, u, P)
    B = B_matrix(x[5])
    x_est = A*x + B*u
    P_est = A*P*A' + B*Q*B'
    K = P_est*H'*inv(H*P_est*H'+R)
    return x_est, P_est, K
end

kalman_predict (generic function with 1 method)

Corrección

En cada instante de muestreo del GPS/magnetómetro, se aplica una corrección por medición al estimador.

• Estimador a posteriori: \(\mathbf{\hat{x}}_k^{+} = \mathbf{\hat{x}}_k^{-} + K_k (\mathbf{\overline{z}}_k-H\mathbf{\hat{x}}_k^{-})\)
• Covarianza a posteriori: \(P_k^{+} = (I-K_k H)P_k^{-}\)

function kalman_correct(x_prior, z, P_prior, K)
    x_est = x_prior + K*(z-H*x_prior)
    P_est = (I-K*H)*P_prior
    return x_est, P_est
end

kalman_correct (generic function with 1 method)

function kalman(t, tasa_GPS, u, z)
    x, P, K = kalman_initialize(t, z)
    for k in 2:length(t)
        x_est, P_est, K_k = kalman_predict(x[:,k-1], u[:,k], P[:,:,k-1])
        if k%tasa_GPS == 0
            x_est, P_est = kalman_correct(x_est, z[:,k], P_est, K_k)
        end
        x[:,k] = x_est
        P[:,:,k] = P_est
        K[:,:,k] = K_k
    end
    return x, P, K
end

kalman (generic function with 1 method)

Estimación de diferentes trayectorias

En todas las trayectorias, los ruidos de los instrumentos se modelan como variables aleatorias gaussianas de media \(0\), con muestras independientes e idénticamente distribuidas.

\[ \mathbf{w}'_k \sim \mathcal{N}\left[\mathbf{0}, Q\right] \qquad \mathbf{v}_k \sim \mathcal{N}\left[\mathbf{0}, R\right] \]

T = 500
t = 0:0.1:T

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

Móvil estático

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} 0 \\ 0 \\ 0 \end{bmatrix} + \mathbf{v}_k \]

function estatico_IMU()
    return [0,0,0]
end
function estatico_GPS()
    return [0,0,0]
end

u_true = hcat([estatico_IMU() for ti in t]...)
z_true = hcat([estatico_GPS() for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

function trayectoria_xy(x, z_true)
    p1 = plot(x[1,:], x[3,:], label="Estimación", xlabel="x", ylabel="y", aspect_ratio=:equal, title="Trayectoria XY")
    plot!(p1, z_true[1,:], z_true[2,:], label="Trayectoria")
    return p1
end

trayectoria_xy(x, z_true)

function trayectoria_t(t, x, z_true)
    p1 = plot(t, x[1,:], ylabel="x [m]", title="Trayectoria en función del tiempo")
    plot!(p1, t, z_true[1,:])
    p2 = plot(t, x[3,:], ylabel="y [m]")
    plot!(p2, t, z_true[2,:])
    p3 = plot(t, x[5,:] .% 2pi, ylabel="θ", xlabel="t [s]")
    plot!(p3, t, z_true[3,:] .% 2pi)
    return plot(p1,p2,p3, layout=(3,1), legend=false)
end

trayectoria_t(t, x, z_true)

Movimiento rectilíneo uniformemente acelerado

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} a \\ 0 \\ 0 \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} \frac 1 2 a t_k^2 \cos(\theta) \\ \frac 1 2 a t_k^2 \sin(\theta) \\ \theta \end{bmatrix} + \mathbf{v}_k \]

function mrua_IMU(a)
    return [a,0,0]
end
function mrua_GPS(a, t, θ)
    return [1/2 * a * t^2 * cos(θ), 1/2 * a* t^2 * sin(θ), θ]
end

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Movimiento circular uniforme

\[ \mathbf{\overline{u}}_k = \begin{bmatrix} 0 \\ \omega^2 r \\ \omega \end{bmatrix} + \mathbf{w}_k' \]

\[ \mathbf{\overline{z}}_k = \begin{bmatrix} r \cos(\omega t_k) \\ r \sin(\omega t_k) \\ \omega t_k + \frac \pi 2 \end{bmatrix} + \mathbf{v}_k \]

function circular_IMU(r, w, t)
    return [0,w^2*r,w] 
end

function circular_GPS(r, w, t)
    return [r*cos(w*t),r*sin(w*t),w*t+π/2] 
end

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

Análisis de desempeño

Error de estimación

Se procede a cuantificar el error de la estimación, el cual se define como la norma cuadrática de la distancia entre la estimación y el valor real

\[ \epsilon_k = \left\lVert\begin{bmatrix}\hat{x}_k\\\hat{y}_k\end{bmatrix}- \begin{bmatrix}z^x_k\\ z^y_k\end{bmatrix}\right\rVert^2 \]

function error_cuadratico(x, z_true)
    xs = [[x_i[1] x_i[3]] for x_i in eachcol(x)]
    zs = [[z_i[1] z_i[2]] for z_i in eachcol(z_true)]
    return [norm(x_i-z_i)^2 for (x_i, z_i) in zip(xs, zs)]
end

error_cuadratico (generic function with 1 method)

Esto se compara con la incerteza del estimador, la cual se cuantifica con la traza de la matriz de covarianza \(P\)

function varianza_estimador(P)
    return [tr(P[:,:,i]) for i in 1:size(P,3)]
end

varianza_estimador (generic function with 1 method)

function plot_error(t, x, z_true, P)
    v = varianza_estimador(P)
    e = error_cuadratico(x, z_true)
    p1 = plot(t, v, ylim=[0, maximum(v)*1.1], ylabel="Incerteza")
    p2 = plot(t, e, ylim=[0, maximum(e)*1.1], ylabel="Error cuadrático", xlabel="t")
    return plot(p1, p2, layout=(2,1))
end

plot_error(t, x, z_true, P)

Ganancia de Kalman

function kalman_gain(K)
    return [norm(K[:,:,i]) for i in 1:size(K,3)]
end

kalman_gain (generic function with 1 method)

function plot_gain(t, P, K)
    v = varianza_estimador(P)
    k = kalman_gain(K)
    p1 = plot(t, v, ylim=[0, maximum(v)*1.1], ylabel="Incerteza")
    p2 = plot(t, k, ylim=[0, maximum(k)*1.1], ylabel="Ganancia Kalman", xlabel="t")
    return plot(p1, p2, layout=(2,1))
end
plot_gain(t, P, K)

k1 = 4000
k2 = 4050
plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])

Efecto de la incerteza de los instrumentos

Incrementa x5 error de GPS

Q = [err_IMU_x^2 0 0; 0 err_IMU_x^2 0; 0 0 err_IMU_θ^2]
R = [(5*err_GPS)^2 0 0; 0 (5*err_GPS)^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

([6.399781199194112 6.3997990738809545 … 45.341589260594674 45.36055364889119; 0.0 0.00035749373686201424 … 0.18985890889783308 0.18942885703251805; … ; 0.0 0.00021385783699033828 … 0.1557259904736974 0.15562015104786917; 0.7848488405492863 0.7849244225793554 … 0.7857678701547156 0.7854209220538022], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … -3.269996546517461e-26 0.0; 0.6250000032 6.250000064 … 4.027810561325809e-24 0.0; … ; 1.5455836360409911e-25 -4.5202712722881835e-24 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 4.860834460651788e-25 0.0; 1.2500000128000002 6.250000128 … 3.802909870361181e-24 0.0; … ; -4.120565610610859e-25 -4.7063484492424615e-24 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … 1.107835787193436e-21 0.0; 0.010048778940915964 0.00011380649969204478 … 2.3689206546853487e-23 0.0; … ; 1.9286714934979128e-21 7.105349407547785e-24 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … 1.1028462603811658e-21 0.0; 0.009946127965829608 0.00011323011782660337 … 2.266065265163026e-23 0.0; … ; 1.8874621364388075e-21 5.740016248312654e-24 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … 1.1049421360554004e-21 0.0; 0.009957454177612268 0.00011329411782660338 … 2.580537468853133e-23 0.0; … ; 1.8877920421337128e-21 7.9209010990109e-24 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 -5.659833239126597e-29 0.0; 0.0038446751446328943 -6.915408885968804e-28 0.0; … ; 9.505566154937311e-28 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 9.813851658963173e-29 0.0; 0.007680491630032021 2.580844671204317e-27 0.0; … ; -2.530169321216744e-27 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -5.024560499576099e-22 0.0; 6.358399722496316e-5 7.001907088186049e-24 0.0; … ; 1.2075519151180111e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -5.010596696256948e-22 0.0; 6.365521898130948e-5 7.016279173975292e-24 0.0; … ; 1.2079757673208367e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -4.885008084320078e-22 0.0; 6.301263777618486e-5 6.984084008668774e-24 0.0; … ; 1.1821788768628917e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

([26.399781199194113 26.399770484752086 … 21.137718470315665 21.139276033568887; 0.0 -0.00021428884050346046 … 0.015848432720687408 0.015302832343740299; … ; 0.0 0.00035418007102216406 … 0.23224185555942428 0.23181349171664725; 1.5702470039467347 1.5715792230382397 … 7.8530947036703145 7.854004392630837], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.31250000016 0.6250000032 … 1.6087096783876353e-28 0.0; 0.6250000032 6.250000064 … 7.256472083863048e-27 0.0; … ; -2.5780275752747166e-28 -3.492159999452689e-27 … 6.250000064 0.0; 0.0 0.0 … 0.0 6.283185334595154;;; 6.5000000016 1.2500000128000002 … 8.119845002821807e-28 0.0; 1.2500000128000002 6.250000128 … 3.938911542442671e-27 0.0; … ; -4.297693240627179e-28 2.5472535823627876e-27 … 6.250000128 0.0; 0.0 0.0 … 0.0 6.283185362010721;;; … ;;; 1.789434126214376 0.010048778940915963 … -1.084727966180855e-21 0.0; 0.010048778940915964 0.00011380649969204478 … -2.2408183550581983e-23 0.0; … ; 4.222681407684344e-21 9.498701955016883e-23 … 0.00011380649969204478 0.0; 0.0 0.0 … 0.0 3.575038139502067e-7;;; 1.7711384781044608 0.009946127965829606 … -1.067773155787924e-21 0.0; 0.009946127965829608 0.00011323011782660337 … -2.2285538000289733e-23 0.0; … ; 4.166316871199004e-21 9.446469265752443e-23 … 0.00011323011782660337 0.0; 0.0 0.0 … 0.0 1.1076370392297255e-7;;; 1.773128836158805 0.009957454177612266 … -1.0700015261885246e-21 0.0; 0.009957454177612268 0.00011329411782660338 … -2.228936724195128e-23 0.0; … ; 4.175763602472809e-21 9.446973240542751e-23 … 0.00011329411782660338 0.0; 0.0 0.0 … 0.0 1.3817927170377632e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.038831218762960626 1.0065668248689088e-31 0.0; 0.0038446751446328943 1.2244906599188314e-30 0.0; … ; -1.5856425077993764e-30 0.0038446751446328943 0.0; 0.0 0.0 0.9999999752491497;;; 0.03993855607702672 7.872090306349743e-31 0.0; 0.007680491630032021 5.335970871632301e-30 0.0; … ; -2.6365881045100373e-30 0.007680491630032021 0.0; 0.0 0.0 0.9999999752491497;;; … ;;; 0.011322706488465957 -6.819089261947242e-22 0.0; 6.358399722496316e-5 -6.907035452039849e-24 0.0; … ; 2.66051354608374e-23 6.358399722496316e-5 0.0; 0.0 0.0 0.6968640780830011;;; 0.011335286259868549 -6.832587527973631e-22 0.0; 6.365521898130948e-5 -6.920990849239914e-24 0.0; … ; 2.666442797567362e-23 6.365521898130948e-5 0.0; 0.0 0.0 0.7122418117142865;;; 0.0112206918646524 -6.693576799048734e-22 0.0; 6.301263777618486e-5 -6.814780531329688e-24 0.0; … ; 2.6313887273236363e-23 6.301263777618486e-5 0.0; 0.0 0.0 0.47048808605167836])

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

plot_error(t, x, z_true, P)

plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])

Incrementa x5 error de IMU

Q = [(5*err_IMU_x)^2 0 0; 0 (5*err_IMU_x)^2 0; 0 0 (5*err_IMU_θ)^2]
R = [err_GPS^2 0 0; 0 err_GPS^2 0; 0 0 err_MAG^2]

dist_ruido_IMU = MvNormal([0,0,0], Q)
dist_ruido_GPS = MvNormal([0,0,0], R)

u_noise = hcat([rand(dist_ruido_IMU) for ti in t]...)
z_noise = hcat([rand(dist_ruido_GPS) for ti in t]...)

u_true = hcat([mrua_IMU(5e-4) for ti in t]...)
z_true = hcat([mrua_GPS(5e-4, ti, pi/4) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

([-4.053193522251121 -4.053107450389068 … 44.57735438453592 44.596830676069956; 0.0 0.0017214372410541373 … 0.19484425526926719 0.19468157541137582; … ; 0.0 -0.0008373128802501247 … 0.23659620376470644 0.23761825355134944; 0.7855433303676169 0.786645576427757 … 0.7857587554823343 0.7868058466499893], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … 9.529706604467709e-24 0.0; 0.62500008 6.2500016 … 1.572470425413841e-23 0.0; … ; 5.5265649441002705e-24 2.8199545226197805e-24 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … 1.7739246973428253e-23 0.0; 1.25000032 6.2500032 … 7.593671972830343e-23 0.0; … ; 2.3009522671901827e-26 -1.2076188861814014e-22 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -1.8396903206497448e-21 0.0; 0.010229584529472202 0.0005721606635760474 … 6.823817069246597e-23 0.0; … ; 2.3993859677098043e-21 2.6112634200199806e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -1.6289303424767417e-21 0.0; 0.009721119932251493 0.0005577606635759174 … 7.340192039930846e-23 0.0; … ; 2.190209028955388e-21 2.7648033253749534e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -1.6190272842674518e-21 0.0; 0.009776975998609085 0.0005593606635759175 … 6.176572960357224e-24 0.0; … ; 2.2184080698883454e-21 3.855958732282835e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 4.044407862005418e-27 0.0; 0.049751250133412535 -2.7043607576388794e-25 0.0; … ; 4.405483851296487e-25 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 3.5591829840556046e-26 0.0; 0.09803924047673965 2.970478437021516e-25 0.0; … ; 4.032043718105931e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -9.921045094305651e-21 0.0; 0.001547195962072148 -2.707088229923479e-22 0.0; … ; 3.4665418277225106e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -9.968180513867565e-21 0.0; 0.0015553791891602389 -2.6961327433362486e-22 0.0; … ; 3.5043344463286204e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -8.998144244783633e-21 0.0; 0.0014823282274877797 -2.4044713562199325e-22 0.0; … ; 3.2221526381863383e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

u_true = hcat([circular_IMU(20, 2*pi/T, ti) for ti in t]...)
z_true = hcat([circular_GPS(20, 2*pi/T, ti) for ti in t]...)

u = u_true + u_noise
z = z_true + z_noise

x, P, K = kalman(t, 10, u, z)

([15.94680647774888 15.946881152223028 … 20.167144093856205 20.166095811845853; 0.0 0.001493489482977554 … -0.009906401051208671 -0.011059239155777875; … ; 0.0 0.0005751244837655708 … 0.3073835570019056 0.3079425585645642; 1.5709414937650652 1.5733003768866411 … 7.853085588997933 7.855389317227024], [6.25 0.0 … 0.0 0.0; 0.0 6.25 … 0.0 0.0; … ; 0.0 0.0 … 6.25 0.0; 0.0 0.0 … 0.0 6.283185307179586;;; 6.312500004 0.62500008 … -1.1829558670837016e-27 0.0; 0.62500008 6.2500016 … 1.5743550327490137e-26 0.0; … ; -1.5598932487744637e-27 -2.1465242046224327e-26 … 6.2500016 0.0; 0.0 0.0 … 0.0 6.2831859925687805;;; 6.500000040000001 1.25000032 … -1.224802040086607e-26 0.0; 1.25000032 6.2500032 … -3.2902646932283077e-26 0.0; … ; -1.31818563583822e-26 -3.978384850805885e-26 … 6.2500032 0.0; 0.0 0.0 … 0.0 6.283186677957975;;; … ;;; 0.36169288198748656 0.010229584529472202 … -6.6133369889639375e-21 0.0; 0.010229584529472202 0.0005721606635760474 … -4.308123134440125e-22 0.0; … ; 5.50852070862659e-21 3.353908489802122e-22 … 0.0005721606635760474 0.0; 0.0 0.0 … 0.0 6.320639916249882e-6;;; 0.3437392039718934 0.009721119932251493 … -6.115763448215439e-21 0.0; 0.009721119932251493 0.0005577606635759174 … -4.103956056185978e-22 0.0; … ; 5.070884373413702e-21 3.187089130763981e-22 … 0.0005577606635759174 0.0; 0.0 0.0 … 0.0 1.5213716556903334e-7;;; 0.34568900956497944 0.009776975998609085 … -6.156801209714805e-21 0.0; 0.009776975998609085 0.0005593606635759175 … -4.104388227819965e-22 0.0; … ; 5.1027683570758455e-21 3.187561317863888e-22 … 0.0005593606635759175 0.0; 0.0 0.0 … 0.0 8.375263600891277e-7], [0.0 0.0 0.0; 0.0 0.0 0.0; … ; 0.0 0.0 0.0; 0.0 0.0 0.0;;; 0.5024875623474667 2.1229816298321444e-31 0.0; 0.049751250133412535 1.2032831145192752e-28 0.0; … ; -1.2414004669719228e-28 0.049751250133412535 0.0; 0.0 0.0 0.9999999752491522;;; 0.5098039231064976 -8.354453207068883e-30 0.0; 0.09803924047673965 -3.131300859215219e-28 0.0; … ; -1.0351894275620513e-27 0.09803924047673965 0.0; 0.0 0.0 0.999999975249155;;; … ;;; 0.054705033709726854 -1.678954910837624e-20 0.0; 0.001547195962072148 -9.830832489442495e-22 0.0; … ; 8.069179262857482e-22 0.001547195962072148 0.0; 0.0 0.0 0.9759866455818303;;; 0.05499827263550295 -1.696971285252791e-20 0.0; 0.0015553791891602389 -9.88833768490497e-22 0.0; … ; 8.113414997461925e-22 0.0015553791891602389 0.0; 0.0 0.0 0.9782848225201974;;; 0.05241135673068665 -1.541475971729123e-20 0.0; 0.0014823282274877797 -9.19117477450877e-22 0.0; … ; 7.507072213514851e-22 0.0014823282274877797 0.0; 0.0 0.0 0.8433959372095443])

trayectoria_xy(x, z_true)

trayectoria_t(t, x, z_true)

plot_error(t, x, z_true, P)

plot_gain(t[k1:k2], P[:,:,k1:k2], K[:,:,k1:k2])